/**
 * @function  接受LORA基站输出的数据，并发布到/joy话题上
 * 
*备注： 
 * 
 * maker:crp
 * 2020-7-5
 * 
 * 方向盘位于 ch0 范围是 [-1,1] 顺时针为减小，逆时针为增加
 * 离合器位于 ch1 范围是 [-1,1]  ，默认状态位 -1   踩下到底为1
 * 油门 位于 ch2 范围是 [-1,1]  ，默认状态位 -1   踩下到底为1
 * 刹车 位于 ch3 范围是 [-1,1]  ，默认状态位 -1   踩下到底为1
 * 方向盘上的方位按键   左右通道位于 ch4 范围是 [-1,0，1]  离散值，默认状态位 0   方向左为1 右为 -1
 * 方向盘上的方位按键   上下通道位于 ch5 范围是 [-1,0，1]  离散值，默认状态位 0  方向上为1  下为 -1
 * 
 *  buttons按键 [0]  "X"  按键  默认为 0    按下为 1
 *  buttons按键 [1]  "□" 按键  默认为 0    按下为 1
 *  buttons按键 [2]  "○" 按键  默认为 0    按下为 1
 *  buttons按键 [3]  "△" 按键  默认为 0    按下为 1
 *  buttons按键 [4]  右侧拨片  默认为 0    按下为 1
 *  buttons按键 [5]  左侧拨片 默认为 0    按下为 1
  *  buttons按键 [6]  R2按键  默认为 0    按下为 1
 *  buttons按键 [7]   L2按键  默认为 0    按下为 1
 *  buttons按键 [8]  SHARE按键  默认为 0    按下为 1
 *  buttons按键 [9]   OPTIONS按键  默认为 0    按下为 1
 *  buttons按键 [10]  R3按键  默认为 0    按下为 1
 *  buttons按键 [11]   L3按键  默认为 0    按下为 1
 * 
  * buttons按键 [12]    档位 左前  默认为 0    按下为 1
 *  buttons按键 [13]   档位 左后  默认为 0    按下为 1
 *  buttons按键 [14]   档位 中前  默认为 0    按下为 1
 *  buttons按键 [15]   档位 中后  默认为 0    按下为 1
 *  buttons按键 [16]   档位 右前  默认为 0    按下为 1
 *  buttons按键 [17]   档位 右后  默认为 0    按下为 1
 * 
 *  buttons按键 [19]   按键 “+”  默认为 0    按下为 1
 *  buttons按键 [20]   档位 “-”   默认为 0    按下为 1
 * 
 *  buttons按键 [23]   档位 红色圆圈“左转按键”   默认为 0    按下为 1
 */


#include<iostream>
#include<string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <vector>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h> 
#include <std_msgs/String.h>

#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>

#include <serial/serial.h>
#include <std_msgs/String.h>

#include <boost/asio.hpp>                  //包含boost库函数
#include <boost/bind.hpp>
#include <sys/time.h>

#include <iostream>
#include <fstream>
#include <sstream>

using namespace std;
using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作


unsigned int speed_0ffset=10000; //偏移量 10000 把数据变换到0-20000
unsigned int KP=10000; //放大系数
 
 

typedef struct{
uint8_t cmd_type;  
uint32_t frame_counter;
uint16_t ch0,ch1,ch2,ch3,ch4,ch5,ch6,ch7;
uint8_t gear;  //档位
uint8_t tem1_buttons,tem2_buttons; 
}LORA;

// typedef struct{
// uint8_t cmd_type;  
// uint32_t frame_counter;
// uint16_t ch0,ch1,ch2,ch3,ch4,ch5,ch6,ch7;
// uint8_t gear;  //档位
// uint8_t tem1_buttons,tem2_buttons; 
// }LORA;
LORA lora;
serial::Serial ros_ser;
ros::Publisher   pub_joy;

bool  analy_uart_recive_data( std_msgs::String serial_data);
void publish_odomtery(void);

int main(int argc,char** argv)
{
	string pub_joy_topic,dev;
	int buad,time_out,hz;

	ros::init(argc, argv, "lora_server");
	ros::NodeHandle n("~");
	 
	n.param<std::string>("pub_joy_topic", pub_joy_topic, "/G29/joy");
	n.param<std::string>("dev", dev, "/dev/ttyUSB0");
	n.param<int>("buad", buad, 115200);
	n.param<int>("time_out", time_out, 1000);
	n.param<int>("hz", hz, 50);

	ROS_INFO_STREAM("pub_joy_topic:   "<<pub_joy_topic);
	ROS_INFO_STREAM("dev:   "<<dev);
	ROS_INFO_STREAM("buad:   "<<buad);
	ROS_INFO_STREAM("time_out:   "<<time_out);
	ROS_INFO_STREAM("hz:   "<<hz);
	// 开启串口模块
	 try
	 {
	    ros_ser.setPort(dev);
	    ros_ser.setBaudrate(buad);
	    //ros_serial::Timeout to = serial::Timeout::simpleTimeout(1000);
	    serial::Timeout to = serial::Timeout::simpleTimeout(time_out);
	    ros_ser.setTimeout(to);
	    ros_ser.open();
	    ros_ser.flushInput(); //清空缓冲区数据
	 }
	 catch (serial::IOException& e)
	 {
	      ROS_ERROR_STREAM("Unable to open port ");
	      return -1;
	}
	if(ros_ser.isOpen())
	{
	   ros_ser.flushInput(); //清空缓冲区数据
	    ROS_INFO_STREAM("Serial Port opened");
	}
	else
	{
	    return -1;
	}
 
   
	//发布主题command
	pub_joy= n.advertise<sensor_msgs::Joy>(pub_joy_topic, 20); //定义要发布/odom主题
	ros::Rate loop_rate(hz);
 
	 while(ros::ok())
	 {
		  //发送帧和接受帧的都以32字节为一个长度包
		  while(ros_ser.available()<32)
			 {
			   //ROS_INFO_STREAM("wait"); 
			   sleep(0.001); //延时0.1秒,确保有数据进入
			}       
			 if(ros_ser.available() )
			 {
			    //ROS_INFO_STREAM("Reading from serial port");
			    std_msgs::String serial_data;
			    //获取串口数据
			    serial_data.data = ros_ser.read(ros_ser.available());
			   bool uart_recive_flag = analy_uart_recive_data(serial_data);
			   if(uart_recive_flag)
			   {
				   publish_odomtery();
					 ROS_INFO_STREAM("Successed analys the data");
			  }
			   else
			   {
			      //serial_data.data = ros_ser.read(ros_ser.available());
			    ros_ser.flushInput(); //清空缓冲区数据
			    //sleep(0.5);            //延时0.1秒,确保有数据进入
			   }
			}
                ros::spinOnce();
                loop_rate.sleep();
	}

	std::cout<<" EXIT ..."<<std::endl;
	ros::waitForShutdown();
	ros::shutdown();

	return 1;
}
/**
 * @function 解析串口发送过来的数据帧
 * 成功则返回true　否则返回false
 */
bool  analy_uart_recive_data( std_msgs::String serial_data)
{
  uint8_t reviced_tem[500];
  uint16_t len=0,i=0,j=0;
  uint8_t check=0;
  uint8_t tem_last=0,tem_curr=0,rec_flag=0;//定义接收标志位
  uint16_t header_count=0,step=0; //计数这个数据序列中有多少个帧头
  len=serial_data.data.size();
  if(len<1 || len>500)
  {
	ROS_INFO_STREAM("serial data is too short ,  len: " << serial_data.data.size() );
     return false; //数据长度太短　
   }
   ROS_INFO_STREAM("Read: " << serial_data.data.size() );

   // 有可能帧头不在第一个数组位置
  for( i=0;i<len;i++) 
  {
	 tem_last=  tem_curr;
	 tem_curr = serial_data.data.at(i);
	 if(tem_last == 0xAE && tem_curr==0xEA&&rec_flag==0) //在接受的数据串中找到帧头　
	 {
		 rec_flag=1;
		 reviced_tem[j++]=tem_last;
		 reviced_tem[j++]=tem_curr;
		 ROS_INFO_STREAM("found frame head" ); 
	}
	else if (rec_flag==1)
	{
		reviced_tem[j++]=serial_data.data.at(i);
		if(tem_last == 0xEF && tem_curr==0xFE)
		{
			header_count++;
			rec_flag=2;
		}
	}
	else
		rec_flag=0;
  }
  // 检验数据长度和校验码是否正确
//   if(reviced_tem[len-3] ==check || reviced_tem[len-3]==0xff)
//     ;
//   else
//     return;
  // 检验接受数据的长度
  step=0;
  for(i=0;i<header_count;i++) 
  {
	  len = (reviced_tem[2+step] ) ; //第一个帧头的长度
	  ROS_INFO_STREAM("read head :" <<i<< "      len:   "<<len);  
	   if(reviced_tem[0+step] ==0xAE && reviced_tem[1+step] == 0xEA && reviced_tem[len-2+step]==0xEF &&reviced_tem[len-1+step]==0xFE) 
      {  //检查帧头帧尾是否完整
		  if (reviced_tem[3+step] ==0x01 )
		  {
 				    uint8_t index_i=3;
					ROS_INFO_STREAM("recived LORA  data" ); 
					lora.cmd_type=reviced_tem[step+3];
	 
					lora.frame_counter=reviced_tem[step+5]*65536*256+reviced_tem[step+5]*65536
																+reviced_tem[step+6]*256+reviced_tem[step+7];
																
					lora.ch0=reviced_tem[step+8]*256+reviced_tem[step+9];
					lora.ch1=reviced_tem[step+10]*256+reviced_tem[step+11];
					lora.ch2=reviced_tem[step+12]*256+reviced_tem[step+13];
					lora.ch3=reviced_tem[step+14]*256+reviced_tem[step+15];
					lora.ch4=reviced_tem[step+16]*256+reviced_tem[step+17];
					lora.ch5=reviced_tem[step+18]*256+reviced_tem[step+19];
					lora.ch6=reviced_tem[step+20]*256+reviced_tem[step+21];
					lora.ch7=reviced_tem[step+22]*256+reviced_tem[step+23];
					lora.gear=reviced_tem[step+24];		
					lora.tem1_buttons=reviced_tem[step+25];		
					lora.tem2_buttons=reviced_tem[step+26];		
					
					ROS_INFO_STREAM("frame_counter:  " << lora.frame_counter);
					ROS_INFO_STREAM("ch0:   "<<  lora.ch0<<"  ch1:   "<<  lora.ch1<<"  ch2:   "<<  lora.ch2<<"  ch3:   "
																<<  lora.ch3<<"  ch4:   "<<  lora.ch4<<"  ch5:   "<<  lora.ch5
																<<"  ch6:   "<<  lora.ch6<<"  ch7:   "<<  lora.ch7);
					ROS_INFO("gear:  %d ",lora.gear);
					ROS_INFO("tem1_buttons:  %X    \t tem2_buttons:  %X  ",lora.tem1_buttons,lora.tem2_buttons);
		  }
			else;
			//return  true;
	  }
	   else
      {
		ROS_WARN_STREAM("frame head is wrong" ); 
         return  false;	
      }
      step+=len; 
  }
 return  true;	         
}

/**
 * @function 发布LORA收到的摇杆数据
 * 
 */
void publish_odomtery(void)
{
	    float tem=0;
		sensor_msgs::Joy  joy;
		
	    tem = float(lora.ch0)/10000.0f-1.0;
		joy.axes.push_back(tem);
	    tem = float(lora.ch1)/10000.0f-1.0;
		joy.axes.push_back(tem);
	    tem = float(lora.ch2)/10000.0f-1.0;
		joy.axes.push_back(tem);
	    tem = float(lora.ch3)/10000.0f-1.0;
		joy.axes.push_back(tem);
	    tem = float(lora.ch4)/10000.0f-1.0;
		joy.axes.push_back(tem);
	    tem = float(lora.ch5)/10000.0f-1.0;
		joy.axes.push_back(tem);
		
		joy.buttons.resize(25); //G29一共发布25个IO状态
		for(int i=0;i<25;i++)
			joy.buttons[i]=0;
		
		//识别方向盘的档位
		if(lora.gear ==1) 
		{
			joy.buttons[12]=1;joy.buttons[13]=0;joy.buttons[14]=0;joy.buttons[15]=0;joy.buttons[16]=0;joy.buttons[17]=0;
		}	
		else if(lora.gear ==2) 
		{
			joy.buttons[12]=0;joy.buttons[13]=1;joy.buttons[14]=0;joy.buttons[15]=0;joy.buttons[16]=0;joy.buttons[17]=0;
		}	
		else if(lora.gear ==3) 
		{
			joy.buttons[12]=0;joy.buttons[13]=0;joy.buttons[14]=1;joy.buttons[15]=0;joy.buttons[16]=0;joy.buttons[17]=0;
		}		
		else if(lora.gear ==4) 
		{
			joy.buttons[12]=0;joy.buttons[13]=0;joy.buttons[14]=0;joy.buttons[15]=1;joy.buttons[16]=0;joy.buttons[17]=0;
		}	
		else if(lora.gear ==5) 
		{
			joy.buttons[12]=0;joy.buttons[13]=0;joy.buttons[14]=0;joy.buttons[15]=0;joy.buttons[16]=1;joy.buttons[17]=0;
		}		
		else if(lora.gear ==6) 
		{
			joy.buttons[12]=0;joy.buttons[13]=0;joy.buttons[14]=0;joy.buttons[15]=0;joy.buttons[16]=0;joy.buttons[17]=1;
		}
		else  
		{
			joy.buttons[12]=0;joy.buttons[13]=0;joy.buttons[14]=0;joy.buttons[15]=0;joy.buttons[16]=0;joy.buttons[17]=0;
		}
		
		joy.buttons[0]=lora.tem1_buttons&0x01;
		joy.buttons[1]=(lora.tem1_buttons>>1) &0x01;
		joy.buttons[2]=(lora.tem1_buttons>>2 ) &0x01;
		joy.buttons[3]=(lora.tem1_buttons>>3 ) &0x01;
		joy.buttons[4]=(lora.tem1_buttons>>4 ) &0x01;
		joy.buttons[5]=(lora.tem1_buttons>>5 ) &0x01;
		joy.buttons[6]=(lora.tem1_buttons>>6 ) &0x01;
		joy.buttons[7]=(lora.tem1_buttons>>7 ) &0x01;

		joy.buttons[8]=lora.tem2_buttons&0x01;
		joy.buttons[9]=lora.tem2_buttons>>1 &0x01;
		joy.buttons[10]=lora.tem2_buttons>>2 &0x01;
		joy.buttons[11]=lora.tem2_buttons>>3 &0x01;
		joy.buttons[19]=lora.tem2_buttons>>4 &0x01;
		joy.buttons[20]=lora.tem2_buttons>>5 &0x01;
		joy.buttons[23]=lora.tem2_buttons>>6 &0x01;

		//发布里程计
		pub_joy.publish(joy);
}
 